package fun.tan90.base.util;

import cn.hutool.core.collection.ListUtil;
import cn.hutool.core.date.DateUtil;
import cn.hutool.core.lang.Tuple;
import fun.tan90.trace.model.AngleVectorData;
import fun.tan90.trace.model.AngleWindowData;

import java.util.Date;
import java.util.List;
import java.util.stream.Collectors;


/**
 * description 角度矢量窗口计算
 * company 铁人科技
 *
 * @author 陈浩杰
 * @date 2024/12/24 18:03
 */
public class WindowAngleUtil {

    /**
     * 角度预处理
     * angle 取值范围0-360
     */
    public static AngleVectorData preCompute(Date ts, double angle) {
        double angleRad = Math.toRadians(angle);
        double angleX = Math.round(Math.sin(angleRad) * 100.0) / 100.0;
        double angleY = Math.round(Math.cos(angleRad) * 100.0) / 100.0;
        return AngleVectorData.builder().ts(ts).angleX(angleX).angleY(angleY).build();
    }

    /**
     * 角度窗口计算
     */
    public static AngleWindowData computeAngleWindowData(List<AngleVectorData> angleVectorDataList) {
        AngleVectorData lastAngleVectorData = angleVectorDataList.get(angleVectorDataList.size() - 1);
        double angleX = angleVectorDataList.stream().mapToDouble(AngleVectorData::getAngleX).average().orElse(0);
        double angleY = angleVectorDataList.stream().mapToDouble(AngleVectorData::getAngleY).average().orElse(0);

        // 反正切函数
        double angle = Math.toDegrees(Math.abs(Math.atan2(angleY, angleX)));

        if (angleX > 0 && angleY > 0) {
            angle = 90 - angle;
        } else if (angleX > 0 && angleY < 0) {
            angle = angle + 90;
        } else if (angleX < 0 && angleY < 0) {
            angle = 90 + angle;
        } else if (angleX < 0 && angleY > 0) {
            angle = 360 + 90 - angle;
        } else if (angleX == 0 && angleY > 0) {
            angle = 0;
        } else if (angleX == 0 && angleY < 0) {
            angle = 180;
        } else if (angleX > 0 && angleY == 0) {
            angle = 90;
        } else if (angleX < 0 && angleY == 0) {
            angle = 270;
        }
        Date ts = lastAngleVectorData.getTs();
        return AngleWindowData.builder().ts(ts).angle(angle).build();
    }

    public static void main(String[] args) {
        // 窗口大小为2
        Date now = DateUtil.beginOfDay(new Date());
        List<Tuple> angles = ListUtil.of(
                new Tuple(DateUtil.offsetSecond(now, 10), 30.0D),
                new Tuple(DateUtil.offsetSecond(now, 11), 210.0D),
                new Tuple(DateUtil.offsetSecond(now, 12), 210.0D)
        );
        List<AngleVectorData> angleVectorDataList = angles.stream().map(n -> {
            Date ts = n.get(0);
            Double angle = n.get(1);
            return WindowAngleUtil.preCompute(ts, angle);
        }).collect(Collectors.toList());
        AngleWindowData angleWindowData = computeAngleWindowData(angleVectorDataList);
        System.out.println("窗口计算后的角度:" + angleWindowData);
    }

}
